#include "rp_motor/eyebrow.h"

// constructor
EyebrowNode::EyebrowNode() :  nh_("~") {
	eyebrows_cstate = eyebrows_fstate =  EYEBROWS_DOWN;
	init();
	extInit();

	motor_sub_ = nh_.subscribe(Topic::MOTOR_BROW, 1, &EyebrowNode::eyebrowCallback, this);
	status_pub_ = nh_.advertise<comm::eyebrows>(Topic::EYEBROWS_STATUS, 10);	// publish status

	// get position of the motor in rad
	motor_->getState();
	motor_pos = motor_->presentPos();
}

// destructor
EyebrowNode::~EyebrowNode() {
	nh_.shutdown();
}

/**
 * Init eyebrow node.
 */
void EyebrowNode::init()
{
	// initialize motor
	ROS_INFO("Initialize eyebrow motor.");

	// read parameters (like the dpr2 robot)
	std::string motor_port_name, motor_config_name;

	ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
	//ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));

	// load motor configuration
	//CXMLConfiguration motor_config_xml;
	//ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	CDxlConfig *config = new CDxlConfig();
	//config.readConfig(motor_config_xml.root().section("motor"));
	motor_ = new C3mxlROS(motor_port_name.c_str());
	//motor_->setConfig(&config);
	motor_->setConfig(config->setID(EYEBROW_PORT));

	// Initialize motor
	ros::Rate init_rate(1);
	while (ros::ok() && motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize eyebrows motor, will continue trying every second");
		init_rate.sleep();
	}

	delete config;
}

/**
 * Moves the motors to the start position.
 */
void EyebrowNode::extInit()
{
	motor_->set3MxlMode(EXTERNAL_INIT);
	motor_->setAcceleration(10);
	motor_->setSpeed(-1);
	motor_->setTorque(-0.5);

	motor_->getStatus();
	while(motor_->presentStatus() == M3XL_STATUS_INITIALIZE_BUSY) {
		motor_->getStatus();// wait
	}
	if(motor_->presentStatus() != M3XL_STATUS_INIT_DONE)
	{
		ROS_ERROR("Error eyebrows: %s", C3mxl::translateErrorCode(motor_->presentStatus()));
	}

	motor_->set3MxlMode(POSITION_MODE);
}

/**
 * Handle incoming messages for the eyebrows.
 * @param	msg	expected state for the eyebrow.
 */
void EyebrowNode::eyebrowCallback(const comm::eyebrows::ConstPtr &msg)
{
	eyebrows_fstate = msg->state;
}

/**
 * Rotate the motor according to the eybrow state.
 */
void EyebrowNode::spin()
{
	// NEUTRAL, UP, DOWN, SPIN

	int spin = -1;	// -1 = not spinning, 0 = down, 1 = up
	ros::Rate loop_rate(20);
	while(ros::ok())
	{
		motor_->getState();
		motor_pos = motor_->presentPos();	// update motor pos

		if(eyebrows_fstate != eyebrows_cstate) // state changed
		{
			switch(eyebrows_fstate)
			{
			case EYEBROWS_SPIN:
				// move motor up and down
				motor_->setPos(0);
				spin = -1;
				break;
			case EYEBROWS_DOWN:
				// move motor up,
				motor_->setPos(0);
				break;
			case EYEBROWS_UP:
				// move motor down
				motor_->setPos(M_PI/2);
				 break;
			case EYEBROWS_NEUTRAL:
				ROS_INFO("Set neutral");
				// set neutral position
				motor_->setPos(M_PI/4);
				break;
			}
			eyebrows_cstate = eyebrows_fstate;
		}
		else
		{
			// do some work
			if(eyebrows_cstate == EYEBROWS_SPIN)
			{
				if(motor_pos >= M_PI/2 && spin != 0)	// half circle
				{
					motor_->setPos(0);
					spin = 0;
				}
				else if(motor_pos <= 0.2 && spin != 1)
				{
					motor_->setPos(M_PI);
					spin = 1;
				}
			}
		}

		comm::eyebrows msg;
		msg.state = eyebrows_cstate;
		status_pub_.publish(msg);

		ros::spinOnce();
		loop_rate.sleep();
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "eyebrow_motor");
	EyebrowNode eyebrow;
	eyebrow.spin();
	return 0;
}
